#!/usr/bin/env python3
# coding: utf-8
import rospy
from std_msgs.msg import Bool
from geometry_msgs.msg import Point
from px4_uwb.msg import Bspline


def make_bspline_msg():
    m = Bspline()
    # 基本标量字段
    m.drone_id = 1
    m.order = 3
    m.traj_id = 1
    m.start_time = rospy.Time(secs=1730197048, nsecs=983585344)

    # knots: 假设是 float64[] 字段
    m.knots = [
        -3.913758911841384, -2.609172607894256, -1.304586303947128, 0.0,
        1.304586303947128, 2.609172607894256, 3.913758911841384, 5.218345215788512,
        6.522931519735639, 7.827517823682767, 9.132104127629894, 10.436690431577022,
        11.74127673552415, 13.045863039471277, 14.350449343418404, 15.655035647365532,
        16.95962195131266, 18.26420825525979
    ]

    # pos_pts: 假设是 geometry_msgs/Point[]
    pts = [
        (-0.0753370988487, 1.0734128612, 0.747853749352),
        (-0.0553499022318, 1.08574489193, 0.729896507472),
        (-0.0348827316242, 1.09749170863, 0.711985742676),
        (0.0390796934767, 1.04159028309, 0.700508345504),
        (0.188988505766, 0.896247754324, 0.69461209597),
        (0.413290670963, 0.667509772208, 0.691916875933),
        (0.694182807475, 0.376709248674, 0.690963261017),
        (1.00493224652, 0.0525341224195, 0.691278272946),
        (1.3153188187, -0.27331433928, 0.692733521239),
        (1.59518536892, -0.568943157006, 0.694989922912),
        (1.81704625858, -0.804610592807, 0.697446891718),
        (1.95914948745, -0.956251045421, 0.699360353865),
        (2.01002504292, -1.01069144706, 0.700130276655),
        (1.97443472658, -0.972667101661, 0.699606370096)
    ]
    m.pos_pts = []
    for x, y, z in pts:
        p = Point()
        p.x = x
        p.y = y
        p.z = z
        m.pos_pts.append(p)

    # yaw_pts: 你给的是空列表
    m.yaw_pts = []

    # yaw_dt:
    m.yaw_dt = 0.0

    return m


class BsplinePublisherNode(object):
    def __init__(self):
        rospy.init_node("uav1_bspline_publisher", anonymous=False)
        self.pub = rospy.Publisher("/uav1/bspline", Bspline, queue_size=5)
        rospy.Subscriber("/uav1/traj_start", Bool, self.traj_start_cb, queue_size=1)

        rospy.loginfo("[uav1_bspline_publisher] node started, waiting for /uav1/traj_start")

    def traj_start_cb(self, msg: Bool):
        if not isinstance(msg, Bool):
            rospy.logwarn("Received non-Bool on /uav1/traj_start; ignoring.")
            return
        if msg.data:
            rospy.loginfo("Received traj_start = True -> publishing Bspline to /uav1/bspline")
            bs = make_bspline_msg()
            # 发布一次。如果需要重复发布或带时间戳/seq，可在这里扩展逻辑。
            self.pub.publish(bs)
            rospy.loginfo("Bspline published.")
        else:
            rospy.loginfo("Received traj_start = False -> no action.")

    def spin(self):
        rospy.spin()


if __name__ == "__main__":
    try:
        node = BsplinePublisherNode()
        node.spin()
    except rospy.ROSInterruptException:
        pass

